﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;
using Drifting.MotionPlanning.Primitives;

namespace Drifting.MotionPlanning.Systems
{
    public class Point2D : BaseSystem
    {
        Random rand;
        public Point2D()
            : base("../../../MotionPlanning/Resources/Point2D - Standard.kdl")
        {
            rand = new Random();
            t_delta = 0.1;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            throw new NotImplementedException();
        }

        override public bool Propagate(MemoryPad pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            double phi = u[0];
            double r = u[1];

            double dx = r * Math.Cos(phi) * duration;
            double dy = r * Math.Sin(phi) * duration;

            if (q_end == null)
                q_end = new double[DOF_State];
            q_end[0] = q_start[0] + dx;
            q_end[1] = q_start[1] + dy;

            step_cost = r;

            return inStateSpace(q_end);
        }
		
		override public Edge Steer(State x_start, State x_end)
        {
            double[] q0 = x_start.Q;
            double[] q1 = x_end.Q;
            double L = MathAssistant.Euclidean(q0, q1);

            Edge e = new Edge();
            e.Dt = L;
            e.control = new double[2];
            e.control[1] = 1;
            e.step_cost = L;

            e.entry = x_start;
            e.exit = x_end;

            return e;
        }

        override public bool ValidateEdge(Edge e)
        {
            double t_inc = 0.05;

            double[] q0 = e.entry.Q;
            double[] q1 = e.exit.Q;

            double edgeLength = MathAssistant.Euclidean(q0, q1);
            int n_inc = (int)(edgeLength / t_inc);

            double[] q_tmp = new double[2];
            for (int i = 1; i <= n_inc; ++i)
            {
                double alpha = i * t_inc / edgeLength;

                q_tmp[0] = q0[0] * alpha + q1[0] * (1 - alpha);
                q_tmp[1] = q0[1] * alpha + q1[1] * (1 - alpha);

                if (collisionFree(q_tmp) == false)
                    return false;
            }

            return true;
        }

        private bool collisionFree(double[] q)
        {
            for (int i = 0; i < SimpleEnv.Count; ++i)
            {
                SimpleRec rec = (SimpleRec)SimpleEnv[i];


                double dx = Math.Abs(q[1] - rec.x);
                double dy = Math.Abs(q[0] - rec.y);

                if (dx <= rec.rx && dy <= rec.ry)
                    return false;
            }

            return true;
        }

        override public bool inStateSpace(double[] q)
        {
            for (int i = 0; i < dof_state; ++i)
            {
                if (q[i] < min_state[i] || q[i] > max_state[i])
                    return false;
            }

            return collisionFree(q);
        }
                
        override public Edge ExecuteControlTo(double[] q_target, double delta, State x_start, double duration)
        {
            //////////////////////////////
            double Kp = 0.25;// 0.25;   //
            bool doSteering = false;    //
            //////////////////////////////

            #region try Steering
            if (doSteering)
            {
                double[]  q_actual = new double[2];
                q_actual[0] = q_target[0];
                q_actual[1] = q_target[1];
                State x_actual = new State(q_actual, q_actual);

                Edge e_actual = Steer(x_start, x_actual);
                x_actual.ParentEdge = e_actual;

                double stepCost_actual = Math.Sqrt(PointSets.BinQuery.Euclidean2(q_actual, x_start.Q));

                e_actual.step_cost = stepCost_actual;
                x_actual.Cost = x_start.Cost + stepCost_actual;

                return e_actual;
            }
            #endregion            
            
            #region try Controller
            int n = (int)Math.Floor(duration / t_delta);
            double[] q_new = new double[2];
            double realCost = 0;

            double x_i = x_start.Q[0];
            double y_i = x_start.Q[1];
            for (int i = 0; i<n;++i )
            {
                double x_e = q_target[0] - x_i;
                double y_e = q_target[1] - y_i;

                double phi_e = Math.Atan2(y_e, x_e);
                double r_e = Math.Sqrt(x_e * x_e + y_e * y_e);

                double noise_angle_max = 15;//degree
                double noise_radian_max = noise_angle_max / 180.0 * Math.PI;
                double noise_phi = rand.NextDouble() * (noise_radian_max * 2) - noise_radian_max;

                double u_phi = phi_e + noise_phi;
                double u_r = r_e * Kp;

                x_i += Math.Cos(u_phi) * u_r * t_delta;
                y_i += Math.Sin(u_phi) * u_r * t_delta;

                realCost += u_r * t_delta;

                q_new[0] = x_i;
                q_new[1] = y_i;

                if (collisionFree(q_new) == false)
                    return null;

                double dist = Math.Sqrt(PointSets.BinQuery.Euclidean2(q_new, q_target));
                if (dist <= delta)
                    break;
            }

            double dist_error = Math.Sqrt(PointSets.BinQuery.Euclidean2(q_new, q_target));
            if (dist_error > delta)
                return null;
            
            Edge e = new Edge();
            e.Dt = duration;
            e.entry = x_start;

            e.exit = new State(q_new, q_new);
            e.step_cost = realCost;
            e.exit.Cost = x_start.Cost + e.step_cost;
            e.exit.ParentEdge = e;
            
            return e;
            #endregion
        }
    }
}
